

#ifndef AGV_DIAGNOSTICS_HPP_
#define AGV_DIAGNOSTICS_HPP_

#include <atomic>
#include <mutex>
#include <string>
#include <unordered_map>
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "diagnostic_updater/diagnostic_updater.hpp"
#include "diagnostic_updater/publisher.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"



namespace agv_diagnostics {
    enum Light_T {
        light_red = 0x11,
        light_yellow = 0x12,
        light_green = 0x13,
        light_red_slow = 0x21,
        light_yellow_slow = 0x22,
        light_green_slow = 0x23,
        light_red_fast = 0x31,
        light_yellow_fast = 0x32,
        light_green_fast = 0x33,
        light_close = 0x60
    };
    // struct diagnostics_conditions_t
    // {
    //     unsigned char level;
    //     std::string message;
    //     std::double min_limit;
    //     std::double* value;
    //     std::double max_limit;
    // };
    class AgvDiagnostics {
    public:
        template <class NodeT>

        AgvDiagnostics(NodeT node, double period = 1.0) {
            updater_ = std::make_shared<diagnostic_updater::Updater>(node);
            updater_->setHardwareID(node->get_name());
            updater_->setPeriod(period);
        }

        ~AgvDiagnostics() {}

        /*该cpp函数是一个诊断模板函数，
        用于更新诊断状态。它接受一个指向
        (* value特定类型值的指针，
        以及min_limit最小和max_limit最大限制值、level级别、
        key键和message消息等参数,stat。
        根据给定的条件，
        函数将更新诊断状态并返回。*/
        template <typename T>
        void diagnostic_templates(
            const T* value,
            double min_limit,
            double max_limit,
            unsigned char level,
            std::string key,
            std::string message,
            diagnostic_updater::DiagnosticStatusWrapper& stat) {

            unsigned char lvl = 0;
            // const std::string s;

            // unsigned char lvl = stat.level;

            if ((value == nullptr) || (min_limit == std::numeric_limits<double>::quiet_NaN()) || (max_limit == std::numeric_limits<double>::quiet_NaN())) {
                lvl = level;
                message = message + " Value is not valid";
                stat.summary(lvl, message);
                return;
            }
            else if (*value < min_limit) {
                lvl = level;
                message = message + "=" + std::to_string(int(*value)) + " is below minimum limit " + std::to_string(min_limit) + ". level = " + std::to_string(int(level));
            }
            else if (*value > max_limit) {
                lvl = level;
                message = message + "=" + std::to_string(int(*value)) + " is above maximum limit " + std::to_string(max_limit) + ". level = " + std::to_string(int(level));
            }
            else {
                message = message + " = " + std::to_string(int(*value)) + " is within limits";
            }
            stat.add(key, *value);
            stat.summary(lvl, message);
            return;
        }

        std::shared_ptr<diagnostic_updater::Updater> updater_;

    private:
        // rclcpp::Node::SharedPtr node_;
    };

    class AgvDiagnosticsProcess : public rclcpp::Node {

    public:
        AgvDiagnosticsProcess() : Node("agv_diagnostics_process") {

            kLogger_ = this->get_logger();
            kLogger_.set_level(rclcpp::Logger::Level::Info);
            subscriber_ = create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
                "/diagnostics", 10, std::bind(&AgvDiagnosticsProcess::diagnosticCallback, this, std::placeholders::_1));

            executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();

            spin_thread_ = std::make_unique<std::thread>(&AgvDiagnosticsProcess::spin, this);
        }

        ~AgvDiagnosticsProcess() {
            executor_->cancel();
            spin_thread_->join();


            executor_.reset();
            spin_thread_.reset();
        }

    private:
        rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr subscriber_;

        std::unique_ptr<std::thread> spin_thread_;

        std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;

        // 串口计数
        void diagnostic_alarm_light(Light_T /*data*/) {

        }

        rclcpp::Logger kLogger_{ rclcpp::get_logger("") };

        void spin() {
            executor_->spin();


            RCLCPP_INFO(kLogger_, "Exiting spin thread...");
        }
        void diagnosticCallback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) {

            int ok_flag = 0, warn_flag = 0, error_flag = 0, stale_flag = 0;
            for (const auto& status : msg->status) {
                // 处理接收到的诊断信息

                // // 打印诊断信息
                // RCLCPP_INFO(kLogger_, "Received diagnostic message: %s", status.message.c_str());

                int level = status.level;

                // 根据诊断级别处理诊断信息
                if (level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
                    // 处理级别为 OK 的诊断信息
                    // RCLCPP_INFO_STREAM(kLogger_, " diagnostic: " << status.message << " " << status.values[0].key << " " << status.values[0].value);
                    // RCLCPP_INFO_STREAM(kLogger_, " diagnostic: " << status.message << " ok");
                    ok_flag++;
                }
                else if (level == diagnostic_msgs::msg::DiagnosticStatus::WARN) {
                    // RCLCPP_INFO_STREAM(kLogger_, "diagnostic: " << status.message << " " << status.values[0].key << " " << status.values[0].value);
                    // 处理级别为 WARN 的诊断信息
                    RCLCPP_INFO_STREAM(kLogger_, " diagnostic: " << status.message << " warn");
                    warn_flag++;
                }
                else if (level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) {
                    // RCLCPP_INFO_STREAM(kLogger_, "diagnostic: " << status.message << " " << status.values[0].key << " " << status.values[0].value);
                    // 处理级别为 ERROR 的诊断信息
                    RCLCPP_INFO_STREAM(kLogger_, " diagnostic: " << status.message << " error");
                    error_flag++;
                }
                else if (level == diagnostic_msgs::msg::DiagnosticStatus::STALE) {
                    // 处理级别为 STALE 的诊断信息
                    RCLCPP_INFO_STREAM(kLogger_, " diagnostic: " << status.message << " stale");
                    stale_flag++;
                }
            }
            if (error_flag > 0) {
                // 处理错误信息
                diagnostic_alarm_light(light_red_fast);
            }
            else if (warn_flag > 0) {
                // 处理警告信息
                diagnostic_alarm_light(light_yellow_slow);
            }
            else if (ok_flag > 0) {
                // 处理正常信息
                diagnostic_alarm_light(light_green);
            }
            else if (stale_flag > 0) {
                // 处理超时信息
                diagnostic_alarm_light(light_close);
            }
            else {
                diagnostic_alarm_light(light_close);
            }
        }
    };
}

#endif // AGV_DIAGNOSTICS_HPP_
